﻿/* 
 * PROJECT: NyARToolkitCS
 * --------------------------------------------------------------------------------
 *
 * The NyARToolkitCS is C# edition NyARToolKit class library.
 * Copyright (C)2008-2012 Ryo Iizuka
 *
 * This work is based on the ARToolKit developed by
 *   Hirokazu Kato
 *   Mark Billinghurst
 *   HITLab, University of Washington, Seattle
 * http://www.hitl.washington.edu/artoolkit/
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as publishe
 * by the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 * 
 * For further information please contact.
 *	http://nyatla.jp/nyatoolkit/
 *	<airmail(at)ebony.plala.or.jp> or <nyatla(at)nyatla.jp>
 * 
 */
using System;
using System.Diagnostics;
namespace jp.nyatla.nyartoolkit.cs.core
{



    /**
     * このクラスには、方程式を解く関数を定義します。
     */
    public class NyAREquationSolver
    {
        /**
         * この関数は、 二次方程式(ax^2+bx+c=0)を解いて、実根のみを返します。
         * @param i_a
         * 係数aの値
         * @param i_b
         * 係数bの値
         * @param i_c
         * 係数cの値
         * @param o_result
         * 解を受け取る配列。2要素以上である事。
         * @return
         * o_resultに格納した実根の数。
         */
        public static int solve2Equation(double i_a, double i_b, double i_c, double[] o_result)
        {
            Debug.Assert(i_a != 0);
            return solve2Equation(i_b / i_a, i_c / i_a, o_result, 0);
        }
        /**
         * この関数は、 二次方程式(x^2+bx+c=0)を解いて、実根のみを返します。
         * @param i_b
         * 係数bの値
         * @param i_c
         * 係数cの値
         * @param o_result
         * 解を受け取る配列。2要素以上である事。
         * @return
         * o_resultに格納した実根の数。
         */
        public static int solve2Equation(double i_b, double i_c, double[] o_result)
        {
            return solve2Equation(i_b, i_c, o_result, 0);
        }
        /**
         * この関数は、 二次方程式(x^2+bx+c=0)を解いて、実根のみを返します。
         * @param i_b
         * 係数bの値
         * @param i_c
         * 係数cの値
         * @param o_result
         * 解を受け取る配列。
         * @param i_result_st
         * o_resultの格納を開始するインデクス番号
         * @return
         * o_resultに格納した実根の数。
         */
        public static int solve2Equation(double i_b, double i_c, double[] o_result, int i_result_st)
        {
            double t = i_b * i_b - 4 * i_c;
            if (t < 0)
            {
                //虚数根
                return 0;
            }
            if (t == 0)
            {
                //重根
                o_result[i_result_st + 0] = -i_b / (2);
                return 1;
            }
            //実根２個
            t = Math.Sqrt(t);
            o_result[i_result_st + 0] = (-i_b + t) / (2);
            o_result[i_result_st + 1] = (-i_b - t) / (2);
            return 2;
        }

        /**
         * この関数は、３次方程式(a*x^3+b*x^2+c*x+d=0)の実根を計算します。	 
         * このコードは、http://aoki2.si.gunma-u.ac.jp/JavaScript/src/3jisiki.htmlを元にしています。
         * @param i_a
         * X^3の係数
         * @param i_b
         * X^2の係数
         * @param i_c
         * X^1の係数
         * @param i_d
         * X^0の係数
         * @param o_result
         * 実根。double[3]を指定すること。
         * @return
         * 返却した実根の数。
         */
        public static int solve3Equation(double i_a, double i_b, double i_c, double i_d, double[] o_result)
        {
            Debug.Assert(i_a != 0);
            return solve3Equation(i_b / i_a, i_c / i_a, i_d / i_a, o_result);
        }
        /**
         * この関数は、３次方程式(x^3+b*x^2+c*x+d=0)の実根を計算します。	 
         * このコードは、http://aoki2.si.gunma-u.ac.jp/JavaScript/src/3jisiki.htmlを元にしています。
         * @param i_b
         * X^2の係数
         * @param i_c
         * X^1の係数
         * @param i_d
         * X^0の係数
         * @param o_result
         * 実根。double[3]を指定すること。
         * @return
         * 返却した実根の数。
         */
        public static int solve3Equation(double i_b, double i_c, double i_d, double[] o_result)
        {
            double tmp, b, p, q;
            b = i_b / (3);
            p = b * b - i_c / 3;
            q = (b * (i_c - 2 * b * b) - i_d) / 2;
            if ((tmp = q * q - p * p * p) == 0)
            {
                // 重根
                q = NyARMath.cubeRoot(q);
                o_result[0] = 2 * q - b;
                o_result[1] = -q - b;
                return 2;
            }
            else if (tmp > 0)
            {
                // 実根1,虚根2
                double a3 = NyARMath.cubeRoot(q + ((q > 0) ? 1 : -1) * Math.Sqrt(tmp));
                double b3 = p / a3;
                o_result[0] = a3 + b3 - b;
                // 虚根:-0.5*(a3+b3)-b,Math.abs(a3-b3)*Math.sqrt(3.0)/2
                return 1;
            }
            else
            {
                // 実根3
                tmp = 2 * Math.Sqrt(p);
                double t = Math.Acos(q / (p * tmp / 2));
                o_result[0] = tmp * Math.Cos(t / 3) - b;
                o_result[1] = tmp * Math.Cos((t + 2 * Math.PI) / 3) - b;
                o_result[2] = tmp * Math.Cos((t + 4 * Math.PI) / 3) - b;
                return 3;
            }
        }



        /**
         * この関数は、4次方程式(ax^4+bx^3+b*cx^2+d*x+e=0)の実根を計算します。	 
         * @param i_a
         * X^4の係数
         * @param i_b
         * X^3の係数
         * @param i_c
         * X^2の係数
         * @param i_d
         * X^1の係数
         * @param i_e
         * X^0の係数
         * @param o_result
         * 実根。double[4]を指定すること。
         * @return
         * 返却した実根の数。
         */
        public static int solve4Equation(double i_a, double i_b, double i_c, double i_d, double i_e, double[] o_result)
        {
            Debug.Assert(i_a != 0);
            double A3, A2, A1, A0, B3;
            A3 = i_b / i_a;
            A2 = i_c / i_a;
            A1 = i_d / i_a;
            A0 = i_e / i_a;
            B3 = A3 / 4;
            double p, q, r;
            double B3_2 = B3 * B3;
            p = A2 - 6 * B3_2;//A2-6*B3*B3;
            q = A1 + B3 * (-2 * A2 + 8 * B3_2);//A1-2*A2*B3+8*B3*B3*B3;
            r = A0 + B3 * (-A1 + A2 * B3) - 3 * B3_2 * B3_2;//A0-A1*B3+A2*B3*B3-3*B3*B3*B3*B3;
            if (q == 0)
            {
                double result_0, result_1;
                //複二次式
                int res = solve2Equation(p, r, o_result, 0);
                switch (res)
                {
                    case 0:
                        //全て虚数解
                        return 0;
                    case 1:
                        //重根
                        //解は0,1,2の何れか。
                        result_0 = o_result[0];
                        if (result_0 < 0)
                        {
                            //全て虚数解
                            return 0;
                        }
                        //実根1個
                        if (result_0 == 0)
                        {
                            //NC
                            o_result[0] = 0 - B3;
                            return 1;
                        }
                        //実根2個
                        result_0 = Math.Sqrt(result_0);
                        o_result[0] = result_0 - B3;
                        o_result[1] = -result_0 - B3;
                        return 2;
                    case 2:
                        //実根２個だからt==t2==0はありえない。(case1)
                        //解は、0,2,4の何れか。
                        result_0 = o_result[0];
                        result_1 = o_result[1];
                        int number_of_result = 0;
                        if (result_0 > 0)
                        {
                            //NC
                            result_0 = Math.Sqrt(result_0);
                            o_result[0] = result_0 - B3;
                            o_result[1] = -result_0 - B3;
                            number_of_result += 2;
                        }
                        if (result_1 > 0)
                        {
                            //NC
                            result_1 = Math.Sqrt(result_1);
                            o_result[number_of_result + 0] = result_1 - B3;
                            o_result[number_of_result + 1] = -result_1 - B3;
                            number_of_result += 2;
                        }
                        return number_of_result;
                    default:
                        throw new NyARRuntimeException();
                }
            }
            else
            {
                //それ以外
                //最適化ポイント:
                //u^3  + (2*p)*u^2  +((- 4*r)+(p^2))*u -q^2= 0
                double u = solve3Equation_1((2 * p), (-4 * r) + (p * p), -q * q);
                if (u < 0)
                {
                    //全て虚数解
                    return 0;
                }
                double ru = Math.Sqrt(u);
                //2次方程式を解いてyを計算(最適化ポイント)
                int result_1st, result_2nd;
                result_1st = solve2Equation(-ru, (p + u) / 2 + ru * q / (2 * u), o_result, 0);
                //配列使い回しのために、変数に退避
                switch (result_1st)
                {
                    case 0:
                        break;
                    case 1:
                        o_result[0] = o_result[0] - B3;
                        break;
                    case 2:
                        o_result[0] = o_result[0] - B3;
                        o_result[1] = o_result[1] - B3;
                        break;
                    default:
                        throw new NyARRuntimeException();
                }
                result_2nd = solve2Equation(ru, (p + u) / 2 - ru * q / (2 * u), o_result, result_1st);
                //0,1番目に格納
                switch (result_2nd)
                {
                    case 0:
                        break;
                    case 1:
                        o_result[result_1st + 0] = o_result[result_1st + 0] - B3;
                        break;
                    case 2:
                        o_result[result_1st + 0] = o_result[result_1st + 0] - B3;
                        o_result[result_1st + 1] = o_result[result_1st + 1] - B3;
                        break;
                    default:
                        throw new NyARRuntimeException();
                }
                return result_1st + result_2nd;
            }
        }
        /**
         * 3次方程式の実根を１個だけ求める。
         * 4字方程式で使う。
         * @param i_b
         * @param i_c
         * @param i_d
         * @param o_result
         * @return
         */
        private static double solve3Equation_1(double i_b, double i_c, double i_d)
        {
            double tmp, b, p, q;
            b = i_b / (3);
            p = b * b - i_c / 3;
            q = (b * (i_c - 2 * b * b) - i_d) / 2;
            if ((tmp = q * q - p * p * p) == 0)
            {
                // 重根
                q = NyARMath.cubeRoot(q);
                return 2 * q - b;
            }
            else if (tmp > 0)
            {
                // 実根1,虚根2
                double a3 = NyARMath.cubeRoot(q + ((q > 0) ? 1 : -1) * Math.Sqrt(tmp));
                double b3 = p / a3;
                return a3 + b3 - b;
            }
            else
            {
                // 実根3
                tmp = 2 * Math.Sqrt(p);
                double t = Math.Acos(q / (p * tmp / 2));
                return tmp * Math.Cos(t / 3) - b;
            }
        }
        /*
            public static void main(String[] args)
            {
                NyAREquationSolver n = new NyAREquationSolver();
                int l=0;
                double[] r = new double[10];
                try{
                    l=n.solve4Equation(1, 9, -18, -68, 120, r);
                }catch(Exception e){
                    e.printStackTrace();
                }
                System.out.println(l);
            }*/
    }
}
